#ifndef FOLLOWRIGHTWALL_H
#define FOLLOWRIGHTWALL_H

//All includes
#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "roboard_drivers/Encoder.h"
#include "roboard_drivers/Motor.h"
#include "saint_nr_9/MotCom.h"
#include "saint_nr_9/IRdata.h"
#include "saint_nr_9/MotCorrect.h"
#include <unistd.h>
#include <math.h>
#include <stdio.h>
#include "StateMachine.h"
#include "MotorControl.h"

void state_update(const std_msgs::Int32::ConstPtr& state);

void state_straight_wall();
void state_corner();
void state_part_hole_front();
void state_hole();
void state_find_wall();
void state_part_wall_front();
void state_part_wall_part_hole();
void state_tag_handle ();
void state_processing_handle();
void state_find_first_wall();
void state_start();
void ir_callback(const saint_nr_9::IRdata::ConstPtr& data);
void state_wowowo();
void state_wowowo_turn();

#define SPEED_SLOW        0.4
#define SPEED_WALL        0.6
#define SPEED_TURN        0.4

#define MIN_DISTANCE_TO_THE_SIDE          5
#define MAX_DISTANCE_TO_THE_SIDE          7
#define MIN_SENSOR_DIFF_TO_STRAIGHTEN_UP  0.5

#define MIN_MIDDLE_DISTANCE_TO_THE_SIDE   4//5.8
#define MAX_MIDDLE_DISTANCE_TO_THE_SIDE   8//7.2
#define MAX_REASONABLE_DISTANCE           10

#define MAX_SENSOR_DIFF_ACCEPTED          5
#define DISTANCE_TO_THE_WALL              6
#define MAX_OFF_OF_DISTANCE               20 

#define MAX_UNSTRAIGHT                    2

#define DISTANCE_BETWEEN_SENSORS 20
#define DIFFERENCE_TO_CORRECT_DISTANCE    2

#define MAX_ROBO_WINKEL                   15

#define MAX_CORRECT_FRONT                 20

// For convenient calling of respective state handling function
typedef void (*state_function_pointer) ();

state_function_pointer state_functions [13];

#endif
